/***************************************************************************
创建者：彭秋明
 开始时间：         : 2017.02.2７
 copyright            : (C) 同川科技
 修改说明：（每次有修改就添加一条，带有 修改人，修改时间，修改描述）

 ***************************************************************************
 *   测试轨迹规划及取点程序运行时间
 * 路径类型分三种：点到点、直线、圆周
 * 每种类型的路径，按照速度规划也分三种：梯形、半梯形、样条
 * 在上述情形下，每种情形分别测试：轨迹规划、取点(Pos)、取点(Vel)、取点(Acc)、取点(Pos+Vel+Acc)
 * 因此，总共需要测试３×３×5种运行时间
*                                                                         *
 ***************************************************************************/

#ifndef TRAJECTORYTEST_HPP
#define TRAJECTORYTEST_HPP

#include "models.hpp"
#include "measuretime.h"
#include <kdl/path_point.hpp>
#include <kdl/path_line.hpp>
#include <kdl/path_circle.hpp>
#include <kdl/path_roundedcomposite.hpp>
#include <kdl/velocityprofile_trap.hpp>
#include <kdl/velocityprofile_traphalf.hpp>
#include <kdl/velocityprofile_spline.hpp>
#include <kdl/trajectory.hpp>

using namespace KDL;

class TrajectoryTest
{
public:
    TrajectoryTest();
    ~TrajectoryTest();

    enum IdentifierType
    {
        ID_LINE=1,
        ID_CIRCLE=2,
        ID_COMPOSITE=3,
        ID_ROUNDED_COMPOSITE=4,
        ID_POINT=5,
        ID_CYCLIC_CLOSED=6,
        TYPE_TRAJ = 7,
        TYPE_POS = 8,
        TYPE_VEL = 9,
        TYPE_ACC = 10,
        TYPE_PVA = 11
    };

    //输出文件，记录轨迹程序测试时间各项数据
    std::ofstream outfile;
    std::string str;

    void pathPointTest();
    void pathLineTest();
    void pathCircleTest();
    void pathRoundedCompositeTest();
    void pathConfigure(Trajectory* traj, TrajectoryTest::IdentifierType testType);
    void getPointTest(MeasureTime* timeTest, Trajectory* traj, TrajectoryTest::IdentifierType testType);
    void writeTrajectoryTestTime(std::string str, MeasureTime* timeTest);

private:
    Chain puma;

};

#endif // TRAJECTORYTEST_HPP
